﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Shapes;
using System.IO.Ports;
using xn;
using TrackingNI;
using System.Threading;
using System.ComponentModel;
using MindDrone.ViewModels;
using MindDrone.Helpers;
using GalaSoft.MvvmLight.Messaging;
using MindDrone.Common;

namespace MindDrone.Views
{
    /// <summary>
    /// Interaction logic for HomeView.xaml
    /// </summary>
    public partial class HomeView : Window
    {
        //TODO - clean up and build a wrapper to move code from here
        #region kinect specific properties 
        private readonly string CONFIG_FILE = @"User.xml";
        private readonly int DPI_X = 96;
        private readonly int DPI_Y = 96;

        private Views.Console console;

        private Context context;
        private ImageGenerator imageGenerator;
        private DepthGenerator depthGenerator;
        private UserGenerator userGenerator;

        private WriteableBitmap imageBitmap;
        private WriteableBitmap depthBitmap;
        private WriteableBitmap depthBitmapCorrected;
        private ImageMetaData imageData;
        private DepthMetaData depthData;

        private PoseDetectionCapability poseDetectionCapability;
        private SkeletonCapability skeletonCapability;

        private SkeletonDraw skeletonDraw;

        private int[] Histogram { get; set; }

        private Thread reader;
        private BackgroundWorker worker;
        private bool stop;
        private uint currentIdUser {get;set;}
        #endregion

        private HomeViewModel viewModel { get; set; }
        public HomeView()
        {
            InitializeComponent();

            this.Loaded += new RoutedEventHandler(HomeView_Loaded);
            this.StateChanged += new EventHandler(HomeView_StateChanged);
            console = new Console();
            console.Show();
            console.Top = 0;
            console.Left = 0;

            if (Settings.UseKinect)
            {
                try
                {
                    context = new Context(CONFIG_FILE);
                    imageGenerator = new ImageGenerator(context);
                    depthGenerator = new DepthGenerator(context);
                    userGenerator = new UserGenerator(context);

                    poseDetectionCapability = userGenerator.GetPoseDetectionCap();
                    skeletonCapability = userGenerator.GetSkeletonCap(); ;

                    MapOutputMode mapMode = depthGenerator.GetMapOutputMode();

                    int width = (int)mapMode.nXRes;
                    int height = (int)mapMode.nYRes;

                    imageBitmap = new WriteableBitmap(width, height, DPI_X, DPI_Y, PixelFormats.Rgb24, null);
                    depthBitmap = new WriteableBitmap(width, height, DPI_X, DPI_Y, PixelFormats.Rgb24, null);
                    depthBitmapCorrected = new WriteableBitmap(width, height, DPI_X, DPI_Y, PixelFormats.Rgb24, null);
                    imageData = new ImageMetaData();
                    depthData = new DepthMetaData();

                    skeletonDraw = new SkeletonDraw();
                    skeletonDraw.MovementDetector.OnMovementChanged += new EventHandler<Kinect.MotionDetection.MovementEventArgs>(MovementDetector_OnMovementChanged);
                    Histogram = new int[depthGenerator.GetDeviceMaxDepth()];

                    reader = new Thread(new ThreadStart(Reader));
                    reader.IsBackground = true;
                    worker = new BackgroundWorker();
                    stop = false;

                    CompositionTarget.Rendering += new EventHandler(Worker);
                    Closing += new System.ComponentModel.CancelEventHandler(MainWindow_Closing);

                    userGenerator.NewUser += new UserGenerator.NewUserHandler(userGenerator_NewUser);
                    userGenerator.LostUser += new UserGenerator.LostUserHandler(userGenerator_LostUser);
                    skeletonCapability.CalibrationStart += new SkeletonCapability.CalibrationStartHandler(skeletonCapability_CalibrationStart);
                    skeletonCapability.CalibrationEnd += new SkeletonCapability.CalibrationEndHandler(skeletonCapability_CalibrationEnd);
                    skeletonCapability.SetSkeletonProfile(SkeletonProfile.All);
                    poseDetectionCapability.PoseDetected += new PoseDetectionCapability.PoseDetectedHandler(poseDetectionCapability_PoseDetected);
                    poseDetectionCapability.PoseEnded += new PoseDetectionCapability.PoseEndedHandler(poseDetectionCapability_PoseEnded);

                    reader.Start();
                    worker.DoWork += new DoWorkEventHandler(WorkerTick);
                }
                catch (Exception ex)
                {
                    ErrorLogger.Log(ex);
                    MessageBox.Show("Please verify your kinect is plugged and OpenNi version 1.0.0.25 is installed","Kinect error");
                } 
            }

        }
        private delegate void changeImage();

        void DroneWrapper_OnImageChanged(object sender, ARDroneConnector.DroneEventArgs e)
        {
            Dispatcher.BeginInvoke(new changeImage(() =>
                {
                    imageDrone.Source = e.Image;
                }), null);
        }

        void HomeView_Loaded(object sender, RoutedEventArgs e)
        {
            if (viewModel == null)
            {
                viewModel = this.DataContext as HomeViewModel;
                viewModel.PropertyChanged += new PropertyChangedEventHandler(viewModel_PropertyChanged);
            }
            foreach (string port in SerialPort.GetPortNames().OrderBy(p=>int.Parse(p.Replace("COM",""))))
                cboPort.Items.Add(port);
            cboPort.SelectedIndex = 0;
        }

        void viewModel_PropertyChanged(object sender, PropertyChangedEventArgs e)
        {
            if (e.PropertyName == HomeViewModel.DroneWrapperPropertyName)
            {
                if (viewModel.DroneWrapper != null)
                {
                    //juste besoin d'etre abonne une fois donc on se desanbonne directement apres la premiere fois.
                    viewModel.PropertyChanged -= viewModel_PropertyChanged;
                    //faire un desabonnement pour ne pas avoir des doublons d'abonnements / perte en perf et traitement en plus
                    //celle ci ne leve pas d'exception meme si aucun abonnement a ete fait
                    viewModel.DroneWrapper.OnImageChanged -= DroneWrapper_OnImageChanged;
                    //faire l'abonnement sur le changement d'image renvoye du drone
                    viewModel.DroneWrapper.OnImageChanged += new EventHandler<ARDroneConnector.DroneEventArgs>(DroneWrapper_OnImageChanged);
                }
            }
        }

        //TODO - can be moved into wrapper
        #region kinect specific
        void MovementDetector_OnMovementChanged(object sender, Kinect.MotionDetection.MovementEventArgs e)
        {
            txtMovementStatus.Text = e.Movement.ToString();
            
            if (viewModel.DroneWrapper != null)
            {
                switch (e.Movement)
                {
                    case Kinect.MotionDetection.MovementType.LEFT:
                        viewModel.DroneWrapper.Move(0f, 0f, 0f, -0.4f);
                        break;
                    case Kinect.MotionDetection.MovementType.RIGHT:
                        viewModel.DroneWrapper.Move(0f, 0f, 0f, 0.4f);
                        break;
                    case Kinect.MotionDetection.MovementType.STRAIGHT:
                        viewModel.DroneWrapper.Move(0f, 0f, 0f, 0f);
                        break;
                    case Kinect.MotionDetection.MovementType.STRAIGHT_FORWARD:
                        viewModel.DroneWrapper.Move(0f, -0.2f, 0f, 0f);
                        break;
                    case Kinect.MotionDetection.MovementType.STRAIGHT_BACKWARD:
                        viewModel.DroneWrapper.Move(0f, 0.2f, 0f, 0f);
                        break;
                    case Kinect.MotionDetection.MovementType.STRAIGHT_UPWARD:
                        viewModel.DroneWrapper.Move(0f, 0f, 0.4f, 0f);
                        break;
                    case Kinect.MotionDetection.MovementType.STRAIGHT_DOWNWARD:
                        viewModel.DroneWrapper.Move(0f, 0f, -0.3f, 0f);
                        break;
                    case Kinect.MotionDetection.MovementType.NOTHING:
                        viewModel.DroneWrapper.Move(0f, 0f, 0f, 0f);
                        break;
                    case Kinect.MotionDetection.MovementType.GROUPING_HAND:
                        viewModel.DroneWrapper.Land();
                        break;
                    default:
                        break;
                }  
            }
            
        }

        void poseDetectionCapability_PoseEnded(ProductionNode node, string pose, uint id)
        {
            Console.Write(id + " Lost Pose " + id);
        }

        void poseDetectionCapability_PoseDetected(ProductionNode node, string pose, uint id)
        {
            Console.Write(id + " Detected pose " + pose);
            userGenerator.GetPoseDetectionCap().StopPoseDetection(id);
            userGenerator.GetSkeletonCap().RequestCalibration(id, false);
        }

        void skeletonCapability_CalibrationEnd(ProductionNode node, uint id, bool success)
        {
            Console.Write(id + " Calibration ended " + (success ? "successfully" : "unsuccessfully"));
            if (success)
            {
                userGenerator.GetSkeletonCap().StartTracking(id);
                viewModel.DroneTakeOffCommand.Execute(null);
                //save current calibrated user
                currentIdUser = id;
            }
            else
            {
                userGenerator.GetPoseDetectionCap().StartPoseDetection(userGenerator.GetSkeletonCap().GetCalibrationPose(), id);
            }
        }

        void skeletonCapability_CalibrationStart(ProductionNode node, uint id)
        {
            Console.Write(id + " Calibration start");
        }

        void userGenerator_LostUser(ProductionNode node, uint id)
        {
            Console.Write(id + " Lost user");
            if (currentIdUser == id)
                viewModel.DroneLandCommand.Execute(null);
        }

        void userGenerator_NewUser(ProductionNode node, uint id)
        {
            userGenerator.GetPoseDetectionCap().StartPoseDetection(userGenerator.GetSkeletonCap().GetCalibrationPose(), id);
            Console.Write(id + " Found new user");
        }


        private void Reader()
        {
            while (!stop)
            {
                try
                {
                    context.WaitAndUpdateAll();
                    imageGenerator.GetMetaData(imageData);
                    depthGenerator.GetMetaData(depthData);
                }
                catch (Exception) { }
            }
        }

        private void WorkerTick(object sender, DoWorkEventArgs e)
        {
            Dispatcher.BeginInvoke((Action)delegate
            {
                //imgRaw.Source = DepthImageSource;
                imgDepth.Source = DepthImageSourceCorrected;
            });
        }

        private void Worker(object sender, EventArgs e)
        {
            if (!worker.IsBusy)
            {
                worker.RunWorkerAsync();
            }
        }

        private void MainWindow_Closing(object sender, System.ComponentModel.CancelEventArgs e)
        {
            stop = true;
        }

        // thanks to Vangos Pterneas for these functions 
        public unsafe void UpdateHistogram(DepthMetaData depthMD)
        {
            for (int i = 0; i < Histogram.Length; ++i)
                Histogram[i] = 0;

            ushort* pDepth = (ushort*)depthMD.DepthMapPtr.ToPointer();

            int points = 0;
            for (int y = 0; y < depthMD.YRes; ++y)
            {
                for (int x = 0; x < depthMD.XRes; ++x, ++pDepth)
                {
                    ushort depthVal = *pDepth;
                    if (depthVal != 0)
                    {
                        Histogram[depthVal]++;
                        points++;
                    }
                }
            }

            for (int i = 1; i < Histogram.Length; i++)
            {
                Histogram[i] += Histogram[i - 1];
            }

            if (points > 0)
            {
                for (int i = 1; i < Histogram.Length; i++)
                {
                    Histogram[i] = (int)(256 * (1.0f - (Histogram[i] / (float)points)));
                }
            }
        }

        public ImageSource RawImageSource
        {
            get
            {
                if (imageBitmap != null)
                {
                    imageBitmap.Lock();
                    imageBitmap.WritePixels(new Int32Rect(0, 0, imageData.XRes, imageData.YRes), imageData.ImageMapPtr, (int)imageData.DataSize, imageBitmap.BackBufferStride);
                    imageBitmap.Unlock();
                }

                return imageBitmap;
            }
        }

        public ImageSource DepthImageSource
        {
            get
            {
                if (depthBitmap != null)
                {
                    UpdateHistogram(depthData);

                    depthBitmap.Lock();

                    unsafe
                    {
                        ushort* pDepth = (ushort*)depthGenerator.GetDepthMapPtr().ToPointer();
                        for (int y = 0; y < depthData.YRes; ++y)
                        {
                            byte* pDest = (byte*)depthBitmap.BackBuffer.ToPointer() + y * depthBitmap.BackBufferStride;
                            for (int x = 0; x < depthData.XRes; ++x, ++pDepth, pDest += 3)
                            {
                                byte pixel = (byte)Histogram[*pDepth];

                                pDest[0] = pixel;
                                pDest[1] = pixel;
                                pDest[2] = pixel;
                            }
                        }
                    }

                    depthBitmap.AddDirtyRect(new Int32Rect(0, 0, depthData.XRes, depthData.YRes));
                    depthBitmap.Unlock();
                }

                //DepthCorrection.Fix(ref depthBitmap, depthData.XRes, depthData.YRes);
                skeletonDraw.DrawStickFigure(ref depthBitmap, depthGenerator, depthData, userGenerator);

                return depthBitmap;
            }
        }

        public ImageSource DepthImageSourceCorrected
        {
            get
            {
                if (depthBitmapCorrected != null)
                {
                    UpdateHistogram(depthData);

                    depthBitmapCorrected.Lock();

                    unsafe
                    {
                        ushort* pDepth = (ushort*)depthGenerator.GetDepthMapPtr().ToPointer();
                        for (int y = 0; y < depthData.YRes; ++y)
                        {
                            byte* pDest = (byte*)depthBitmapCorrected.BackBuffer.ToPointer() + y * depthBitmapCorrected.BackBufferStride;
                            for (int x = 0; x < depthData.XRes; ++x, ++pDepth, pDest += 3)
                            {
                                byte pixel = (byte)Histogram[*pDepth];

                                pDest[0] = pixel;
                                pDest[1] = pixel;
                                pDest[2] = pixel;
                            }
                        }
                    }

                    depthBitmapCorrected.AddDirtyRect(new Int32Rect(0, 0, depthData.XRes, depthData.YRes));
                    depthBitmapCorrected.Unlock();
                }

                DepthCorrection.Fix(ref depthBitmapCorrected, depthData.XRes, depthData.YRes);
                skeletonDraw.DrawStickFigure(ref depthBitmapCorrected, depthGenerator, depthData, userGenerator);

                return depthBitmapCorrected;
            }
        }
        #endregion

        void HomeView_StateChanged(object sender, EventArgs e)
        {
            if (this.WindowState == System.Windows.WindowState.Maximized)
            {
                //hide everything not needed for fullscreen
                LayoutRoot.RowDefinitions[0].Height = new GridLength(0, GridUnitType.Pixel);
                LayoutRoot.RowDefinitions[1].Height = new GridLength(0, GridUnitType.Pixel);
                LayoutRoot.RowDefinitions[3].Height = new GridLength(0, GridUnitType.Pixel);
                LayoutRoot.RowDefinitions[4].Height = new GridLength(0, GridUnitType.Pixel);
                gaugeMindset.Visibility = System.Windows.Visibility.Collapsed;

                //emphasize the things we need
                //this.WindowState = System.Windows.WindowState.Maximized;
                LayoutRoot.RowDefinitions[2].Height = new GridLength(1, GridUnitType.Star);

                //make smaller kinect image
                gridImages.RowDefinitions.Add(new RowDefinition()
                {
                    Height = new GridLength(0.6, GridUnitType.Star),
                });
                gridImages.RowDefinitions.Add(new RowDefinition()
                {
                    Height = new GridLength(0.4, GridUnitType.Star),
                });
               
                gridImages.HorizontalAlignment = System.Windows.HorizontalAlignment.Stretch;
                imageDrone.SetValue(Grid.RowSpanProperty, 2);
                imageDrone.SetValue(Grid.ColumnSpanProperty, 2);
                imageDrone.MaxHeight = double.PositiveInfinity;
                stackKinect.SetValue(Grid.RowProperty, 1);
                stackKinect.Width = 400;
                imgDepth.Width = 300;
                imgDepth.Height = 300;
                imgDepth.MaxHeight = double.PositiveInfinity;
                stackKinect.Height = 400;
                txtMovementStatus.FontSize = 32;
                txtMovementStatus.Margin = new Thickness(0, -20, 0, 0);

                imgCampudID.SetValue(Grid.RowProperty, 2);
            }
            else
            {
                if (this.WindowState == System.Windows.WindowState.Normal)
                    RestoreState();
            }
        }
        private void RestoreState()
        {
            LayoutRoot.RowDefinitions[0].Height = new GridLength(80, GridUnitType.Star);
            LayoutRoot.RowDefinitions[1].Height = new GridLength(80, GridUnitType.Star);
            LayoutRoot.RowDefinitions[3].Height = new GridLength(30, GridUnitType.Pixel);
            LayoutRoot.RowDefinitions[4].Height = new GridLength(20, GridUnitType.Pixel);
            gaugeMindset.Visibility = System.Windows.Visibility.Visible;

            //emphasize the things we need
            //this.WindowState = System.Windows.WindowState.Maximized;
            LayoutRoot.RowDefinitions[2].Height = new GridLength(414, GridUnitType.Star);

            //make smaller kinect image
            imageDrone.ClearValue(Grid.RowSpanProperty);
            imageDrone.ClearValue(Grid.RowProperty);
            imageDrone.ClearValue(StackPanel.WidthProperty);
            gridImages.RowDefinitions.Clear();
            gridImages.HorizontalAlignment = System.Windows.HorizontalAlignment.Right;
            imageDrone.ClearValue(Grid.ColumnSpanProperty);
            stackKinect.ClearValue(StackPanel.WidthProperty);
            stackKinect.ClearValue(StackPanel.HeightProperty);
            imgDepth.ClearValue(Image.WidthProperty);
            imgDepth.ClearValue(Image.HeightProperty);
            imgDepth.MaxHeight = 320;
            imageDrone.MaxHeight = 320;
            txtMovementStatus.FontSize = 40;
            txtMovementStatus.Margin = new Thickness(0, 0, 0, 0);

            imgCampudID.SetValue(Grid.RowProperty, 0);

        }

    }
}
